# 条件平差 Adjustment of condition equation
import numpy as np

# 观测高差
L = np.mat([12.705, 11.082, 6.518, 4.570, -4.225, 15.302]).transpose()
# 路线长度
S = [1.1, 2.3, 0.8, 1.5, 2.0, 1.9]
# 已知高程
H_A = 788.135
H_B = 811.920
# 权阵
P = np.mat(np.zeros([len(L), len(L)]))
Q = np.mat(np.zeros([len(L), len(L)]))
for i in range(0, len(S)):
	P[i, i] = 1 / S[i]
	Q[i, i] = S[i]
# 协因数阵
# Q = np.linalg.inv(P)
# 条件方程系数阵
A = np.mat([
	[0, -1, 1, 1, 0, 0],
	[0, -1, 0, 0, 1, 1],
	[1, 1, 0, 0, 0, 0]
])
# 条件方程常数向量
A0 = np.mat([0, 0, H_A - H_B]).transpose()
# 闭合差向量
W = A*L + A0
# 法方程系数阵
Naa = A*Q*A.transpose()
# 求解联系数法方程，得到联系数向量K
K = np.linalg.solve(Naa, -W)
# 改正数
V = Q*A.transpose()*K
# 平差值
Lh = L + V

print("观测高差 L = \n", L)
print("条件方程系数阵 A = \n", A)
print("权阵 P = \n", P)
print("协因数阵 Q = \n", Q)
print("闭合差向量 W = \n", W)
print("法方程系数阵 Naa = \n", Naa)
print("联系数向量 K = \n", K)
print("改正数 V = \n", V)
print("平差值 Lh = \n", Lh)
